#include "projection.h"

using namespace std;
using namespace cv;

#define SAMPLE_NEM 8
#define IMG_W 2560
#define IMG_H 1440
#define PI 3.1415926

typedef pcl::PointCloud<pcl::PointXYZRGB> PCLCloud;
typedef pcl::PointXYZRGB PCLPoint;

typedef struct IMGPoints3D {
    float x, y, z, cnt;

    IMGPoints3D() {
        x = 0;
        y = 0;
        z = 0;
        cnt = 0;
    }
} IMGPoints3D;

void SplitString(const string &s, vector<string> &v, const string &c) {
    string::size_type pos1, pos2;
    pos2 = s.find(c);
    pos1 = 0;
    while (string::npos != pos2) {
        v.push_back(s.substr(pos1, pos2 - pos1));

        pos1 = pos2 + c.size();
        pos2 = s.find(c, pos1);
    }
    if (pos1 != s.length())
        v.push_back(s.substr(pos1));
}

int main() {
    // 外参矩阵 4 * 4
    double rtvect[] = {0.0265616, -0.999645, -0.00193387, -0.118765,
                       0.0158947, 0.00235664, -0.999871, -0.0124312,
                       0.999521, 0.0265274, 0.0159516, 0.726805,
                       0, 0, 0, 1};
    // 内参矩阵 3 * 4
    double p_rect[] = {2015.008197939823, 0, 1281.815764715162, 0,
                       0, 2029.382584067923, 788.7069438046608, 0,
                       0, 0, 1, 0};
    // 4 * 4 单位阵
    double r_rect[] = {1, 0, 0, 0,
                       0, 1, 0, 0,
                       0, 0, 1, 0,
                       0, 0, 0, 1};
    //PCL点云指针
    PCLCloud::Ptr cloud_ptr(new PCLCloud);
    //image 3D info
    auto *Info3D = new IMGPoints3D[IMG_W * IMG_H];

    //imread
    Mat image = imread("image_data/img_biaoding.jpg");
    Mat image_mask(image.rows, image.cols, CV_8UC1, cv::Scalar{0});

    pcl::PCLPointCloud2 cloud{};
    pcl::io::loadPLYFile("", cloud);
    pcl::PointCloud<pcl::PointXYZ> local_map;
    pcl::fromPCLPointCloud2(cloud, local_map);

    vector<Point3f> all_points;
    for (auto &i: local_map) {
        all_points.emplace_back(i.x, i.y, i.z);
    }
    //3D points to image projection: R_rect*R_rect*RT*X
    cv::Mat P_rect(3, 4, cv::DataType<double>::type, p_rect);
    cv::Mat R_rect(4, 4, cv::DataType<double>::type, r_rect);
    cv::Mat RT_mat(4, 4, cv::DataType<double>::type, rtvect);
    cv::Mat M_(3, 4, cv::DataType<double>::type);
    cv::Mat X(4, 1, cv::DataType<double>::type);
    cv::Mat Y(3, 1, cv::DataType<double>::type);
    // 3 * 4
    M_ = P_rect * R_rect * RT_mat;

    for (int i = 0; i < all_points.size(); i += 1) {
        // 1. Convert current Lidar point into homogeneous coordinates
        X.at<double>(0, 0) = all_points[i].x;
        X.at<double>(1, 0) = all_points[i].y;
        X.at<double>(2, 0) = all_points[i].z;
        X.at<double>(3, 0) = 1;

        // 2. Apply the projection equation to map X onto the image plane of the camera. Store the result in Y.
        // 3 * 1
        Y = M_ * X;
        // 比例因子
        double scale = Y.at<double>(2, 0);

        // 3. Transform Y back into Euclidean coordinates and store the result in the variable pt.
        cv::Point pt;
        pt.x = (int) (Y.at<double>(0, 0) / scale + 0.5);
        pt.y = (int) (Y.at<double>(1, 0) / scale + 0.5);
        //cout << "pt.x: " << pt.x << " pt.y: " << pt.y << endl;
        if (pt.x < 0 || pt.x > IMG_W) {
            continue;
        }
        if (pt.y < 0 || pt.y > IMG_H) {
            continue;
        }
        // 对应RGB像素位置的坐标
        Info3D[pt.y * IMG_W + pt.x].x += all_points[i].x;
        Info3D[pt.y * IMG_W + pt.x].y += all_points[i].y;
        Info3D[pt.y * IMG_W + pt.x].z += all_points[i].z;
        Info3D[pt.y * IMG_W + pt.x].cnt++;

        //circle(image, pt, 2, Scalar(255, 255, 0), 1, 8, 0);
    }
    for (int i = 0; i < IMG_W * IMG_H; i++) {
        if (Info3D[i].cnt > 0) {
            PCLPoint point_temp;
            // 该处的像素值赋为白色
            image_mask.data[i] = 255;
            Info3D[i].x /= Info3D[i].cnt;
            Info3D[i].y /= Info3D[i].cnt;
            Info3D[i].z /= Info3D[i].cnt;

            point_temp.x = Info3D[i].x;
            point_temp.y = Info3D[i].y;
            point_temp.z = Info3D[i].z;
            point_temp.b = image.data[i * 3];
            point_temp.g = image.data[i * 3 + 1];
            point_temp.r = image.data[i * 3 + 2];
            cloud_ptr->points.push_back(point_temp);
        }
    }

    Mat m_kernel3 = getStructuringElement(MORPH_RECT, Size(3, 3));
    Mat m_kernel5 = getStructuringElement(MORPH_RECT, Size(5, 5));
    dilate(image_mask, image_mask, m_kernel5);
    dilate(image_mask, image_mask, m_kernel3);
    erode(image_mask, image_mask, m_kernel5);

    for (int i = 0; i < IMG_W * IMG_H; i++) {
        if (Info3D[i].cnt == 0 && image_mask.data[i] > 0) {
            int x_s[8] = {-1, -1, 0, 1, 1, 1, 0, -1};
            int y_s[8] = {0, -1, -1, -1, 0, 1, 1, 1};
            // RGB 图像第几列
            int x = i % IMG_W;
            // RGB 图像第几行
            int y = i / IMG_W;
            int depth_t = -1;
            int ref_s = -1;
            for (int j = 0; j < 8; j++) {
                int x_t = x + x_s[j];
                int y_t = y + y_s[j];
                // RGB 像素坐标
                int num_t = y_t * IMG_W + x_t;
                if (Info3D[num_t].cnt > 0 && (abs(Info3D[num_t].z - depth_t) < 100 || depth_t == -1)) {
                    Info3D[i].x += Info3D[num_t].x;
                    Info3D[i].y += Info3D[num_t].y;
                    Info3D[i].z += Info3D[num_t].z;
                    Info3D[i].cnt++;
                    depth_t = Info3D[num_t].z;
                    ref_s = j;
                }
            }
            if (Info3D[i].cnt > 0) {
                // 世界坐标系下，对应同一个RGB坐标时取平均值
                Info3D[i].x /= Info3D[i].cnt;
                Info3D[i].y /= Info3D[i].cnt;
                Info3D[i].z /= Info3D[i].cnt;
                if (Info3D[i].cnt == 1) {
                    Info3D[i].x += 10 * x_s[ref_s];
                    Info3D[i].y += 10 * y_s[ref_s];
                }

                PCLPoint point_temp;
                point_temp.x = Info3D[i].x;
                point_temp.y = Info3D[i].y;
                point_temp.z = Info3D[i].z;
                point_temp.b = image.data[i * 3];
                point_temp.g = image.data[i * 3 + 1];
                point_temp.r = image.data[i * 3 + 2];
                cloud_ptr->points.push_back(point_temp);
            }

        }
    }

    // 设置点云
    cloud_ptr->height = 1;
    cloud_ptr->width = cloud_ptr->points.size();
    cloud_ptr->is_dense = false;

    imshow("image", image);
    imshow("mask", image_mask);
    cv::waitKey(0);
    pcl::visualization::CloudViewer m_viewer("3D Viewer");
    m_viewer.showCloud(cloud_ptr);
    char q;
    std::cin >> q;

//    while (!m_viewer.wasStopped()) {};

    cloud_ptr->points.clear();
    delete[] Info3D;

    return 0;
}
